/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "WirelessMap.h"
#include "glutImageGUI.h"   ///< For visualisation
#include "JPEGEncoder.hpp"  ///< For saving JPEG

void timer(bool isStart){
	static long int ss1,us1;
	long int ss2,us2,sec,us,ms;
	float fms;
		
	if(isStart) ownTime_GetTimeOfDay(&ss1,&us1);
	else{
		ownTime_GetTimeOfDay(&ss2,&us2);
		sec = ss2-ss1;
		us = us2-us1;
		ms = sec*1000+us/1000;
		fms=(float)sec*1000.0+(float)us/1000.0;
		fprintf(stderr,"Time elapsed (%.6fms = %.6fs) \n",fms,fms/1000.0);
	}
}


void printUsage(){
	fprintf(stderr,"\tCreates Heat Map based on one Position/Wireless pair and Localizes with another \n");
	fprintf(stderr,"\t\tReads two logfiles recorded with MaCI_Logger 1) Position and 2) Wireless logfiles \n");
	fprintf(stderr,"\t\tUses WirelessPositionLog Class to read the files \n");
	fprintf(stderr,"\t\tUses given ESSID string to put the signal values into a Wireless heatmap \n");
	fprintf(stderr,"\t\tplots the result\n");
	fprintf(stderr,"\n");
	fprintf(stderr,"USAGE: ./plotWirelessSignalMap -p [name] -w [name] -P [name] -W [ref] -m [name] [opts]\n");
	fprintf(stderr,"\t\t\t[-p name] name of position logfile\n");
	fprintf(stderr,"\t\t\t[-w name] name of the wireless logfile\n");
	fprintf(stderr,"\t\t\t[-P name] name of Reference position logfile\n");
	fprintf(stderr,"\t\t\t[-W name] name of Reference wireless logfile\n");

	fprintf(stderr,"\t\t\t[-e essid]  ESSID that you want to print\n");
	fprintf(stderr,"\t\t\t[-m map]  Line map name\n");
	fprintf(stderr,"\t\t\t[-l] list of Keys (MAC address) with essid\n");
	fprintf(stderr,"\t\t\t[-h/-?]  usage\n\n");
	//fprintf(stderr,"\t\t\t[-l] \n");
}

void saveJPEG(const char *fname, unsigned char *rgb,int Nx, int Ny){
	CJPEGEncoder encoder;
	encoder.SetRGBDataPtr((const char *)rgb,Nx,Ny);
	FILE *f = fopen(fname,"wb");
	fwrite(encoder.GetJPEGDataPtr(), 
				 encoder.GetJPEGDataSize(), 
				 1, f);
	fclose(f);
		
}


int main(int argc, char *argv[]){
	int verbose = 1;
	std::string positionLogfile;
	std::string wirelessLogfile;
	std::string referencepositionLogfile;
	std::string referencewirelessLogfile;
	
	std::string linemapname;
	
	// Init Debug lib
	debugInit();
	debugSetGlobalDebugLvl(verbose);
	bool hasref=false;		
	// Parse parameters
	bool parse = true;
	while(parse) {
		int c;
		c = getopt(argc, argv, "P:W:p:w:e:m:hlvq");
		switch(c) {
			case 'p': {
				positionLogfile = optarg;
				fprintf(stderr,"Using %s as Position logfile\n",
								positionLogfile.c_str());
				break;
			}
			case 'w': {
				wirelessLogfile = optarg;
				fprintf(stderr,"Using %s as wireless logfile\n",
								wirelessLogfile.c_str());
				break;
			}
			case 'P': {
				referencepositionLogfile = optarg;
				fprintf(stderr,"Using %s as Position Reference logfile\n",
								referencepositionLogfile.c_str());
				hasref=true;
				break;
			}
			case 'W': {
				referencewirelessLogfile = optarg;
				fprintf(stderr,"Using %s as wireless Reference logfile\n",
								referencewirelessLogfile.c_str());
				hasref = true;
				break;
			}
			
			case 'm': {
				linemapname = optarg;
				fprintf(stderr,"Using %s as Line map Name\n",linemapname.c_str());
				break;
			}   
			case 'v':
				debugSetGlobalDebugLvl(++verbose);
				break;
      
			case 'q':
				verbose = 0;
				debugSetGlobalDebugLvl(0);
				break;
      
			case 'h':
			case '?':
				printUsage();
				exit(1);
				break;
      
			case -1:
				parse = false;
				break;
		}
	}
	
	dPrint(1,"Logfiles: Position = %s Wireless = %s",
				 positionLogfile.c_str(),wirelessLogfile.c_str());
	
	CWirelessPositionLog log(wirelessLogfile, positionLogfile);
	
	CWirelessMap wmap(log,linemapname, 0.5);
	int Nx,Ny;
	wmap.getDim(Nx,Ny);
	
	unsigned char *rgb = (unsigned char *) malloc(Nx*Ny*3);
	addImageWindow(Nx, Ny);
	glutImageGUI_Start();
	///Display signal maps for each ID
	for(unsigned int i=0; i<log.signalKeys.size();i++){
		wmap.toRGB(log.signalKeys[i], rgb);		
		displayImage(0, rgb,Ny,Nx);
		ownSleep_ms(3500);
	}
	
	dPrint(1,"Reffiles: Position = %s Wireless = %s",
				 referencepositionLogfile.c_str(),referencewirelessLogfile.c_str());
	CWirelessPositionLog reflog(referencewirelessLogfile, referencepositionLogfile);
	
	
	MaCI::Position::CPositionData pos;
	for(EACH_IN_i(reflog.wirelessData)){
		reflog.iPositionBacklog.PeekData(pos, (*i)->GetTimestamp()->GetGimTime(), NULL);
		const MaCI::Position::TPose2D *p = pos.GetPose2D();
		timer(true);
		wmap.computeWirelessMeasurementCost((*i));
		
		wmap.costToRGB(rgb);
		timer(false);
		Grid::TPixel pix= wmap.grid->toPixel(Grid::TCoord2D(p->x,p->y));
		int ind = pix.x+pix.y*Nx;
		
		//fprintf(stderr,"%f, %f, %d,%d  %d\n",p->x,p->y,pix.x,pix.y,ind);
		rgb[3*ind]=0;
		rgb[3*ind+1]=255;
		rgb[3*ind+2]=0;
		
		displayImage(0, rgb,Ny,Nx);
		ownSleep_ms(250);
	}
	
	return 0;
}
